import rospy
import std_msgs
import std_msgs.msg
import sys, select, termios, tty

settings = termios.tcgetattr(sys.stdin)

def Reset_Terminal():
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

def getKey():
    """
    非阻塞读取键盘输入
    """
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    # termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

class newBeeFoot:
    def __init__(self,foot_name:str):
        topic_name="/newBee_gazebo/"+foot_name+"_foot_controller/command"
        self.__pub=rospy.Publisher(topic_name,std_msgs.msg.Float64,queue_size=5)
        self.__vel_cmd=std_msgs.msg.Float64()
        self.__vel_cmd.data=float(0)    # 初始化脚速度为0

    def set_foot_vel(self,vel:float=0):
        """
        设置脚的速度
        """
        self.__vel_cmd.data=vel

    def publish_foot_vel(self):
        """
        发布脚的速度
        """
        self.__pub.publish(self.__vel_cmd)


def main():

    rospy.init_node("foot_cmd_publisher")
    print("init foot_cmd_publisher node")

    # 创建四轮速度发布者实例
    FR_Foot=newBeeFoot("FR")    # 0
    FL_Foot=newBeeFoot("FL")    # 1
    RR_Foot=newBeeFoot("RR")    # 2
    RL_Foot=newBeeFoot("RL")    # 3
    feet=[FR_Foot,FL_Foot,RR_Foot,RL_Foot]

    loop_mgr=rospy.Rate(200)     # 循环频率为10Hz

    default_vel=10    # 默认脚速度为10
    current_speed=0
    print("default speed change to {}".format(default_vel))

    enable_damp=False       # 是否使能减速
    percent=float(0)        # 线性减速百分比
    step_num=50            # 线性减速步数,步数越多,减速越慢
    step_len=float(1/step_num)     # 线性减速步长

    try:
        while(True):
            key=getKey()
            if(key!=''):
                percent=0    # 线性减速百分比归零
                if(key in ["w","s"]):
                    if(key=="w"):
                        current_speed=default_vel    
                    elif(key=="s"):
                        current_speed=-default_vel
                    for foot in feet:
                        foot.set_foot_vel(current_speed)    # 设置脚速度

                elif(key in ["i","k"]):
                    if(key=="i"):
                        default_vel+=1
                    elif(key=="k"):
                        default_vel-=1
                    Reset_Terminal()
                    print("default speed change to {}".format(default_vel))
            else:
                # 线性减速
                if(enable_damp==True):
                    percent+=step_len
                    percent=min(percent,1)  # 截止为1
                    speed=current_speed*(1-percent)
                    for foot in feet:
                        foot.set_foot_vel(speed)    # 设置脚速度
                else:    
                    for foot in feet:
                        foot.set_foot_vel(0)    # 脚速度归零

            for foot in feet:
                foot.publish_foot_vel()    # 发布脚速度
            loop_mgr.sleep()


    except Exception as e:
        Reset_Terminal()
        print(e)

    Reset_Terminal()
    print("foot_cmd_publisher node exit!")


if(__name__=="__main__"):
    main()